/*LICENSE_BEGIN
 * ******************************************************************
 * Copyright 2006 by the Dainamite Robocup-Team at the
 * DAI-Labor, TU-Berlin (www.dai-labor.de)
 * 
 * Members of Dainamite are:
 * Holger Endert
 * Robert Wetzker
 * Thomas Karbe
 * Axel Heßler
 * Philippe Bütter
 * Felix Brossmann
 * 
 * Licensed under the GNU GPL (General Public License) Free Software 
 * License, Version 2. You may use this file only in compliance with 
 * the License. You may obtain a copy of the License at:
 * 
 * http://www.fsf.org/licensing/licenses/gpl.txt
 * ******************************************************************
LICENSE_END*/
package robocup.component.situations;

import java.util.Arrays;

import robocup.component.ControllerNumbers;
import robocup.component.DAInamiteConstants;
import robocup.component.PConf;
import robocup.component.WorldModel;
import robocup.component.ControllerNumbers.INTERCEPTMODE;
import robocup.component.actions.DashAction;
import robocup.component.actions.TurnAction;
import robocup.component.geometry.Vektor;
import robocup.component.worldobjects.Player;

/**
 * This class stores precalculated data about player movements and some
 * methods which refer to this data. The data stored is the max. possible
 * player movement for a given player, a given time and a given delay (i.e. for 
 * turning first). The movement's data is updated each time the player's 
 * effort/stamina/recovery changes by much.<br>
 * <br> 
 * FIXME
 * 
 * - when a player already has a certain initial speed in the same direction,
 * adding his speed to the value stored here might result in a velocity higher 
 * than {@link PConf#PLAYER_SPEED_MAX} leading to wrong predictions.
 */
public class Movements {

	WorldModel world;

	private Player[] teammates;

	private Player[] opponents;

  //the max horizont for precalculation of the movements
	private int maxCycles = DAInamiteConstants.PROPHET_MAX_CYCLES_INTERCEPTION;

	private double[] lastStamina = new double[22];

  //dash100 action
	private DashAction dash100 = new DashAction(100);

  //dash100 backward action
	private DashAction dash100BW = new DashAction(-100);

	// 179 degree for security, don't set it to 180 degree without changing the 
  // methods (otherwise you will see strange things happening ;-)
	private TurnAction turn180 = new TurnAction(179);

	// init players so we don't have to clone later
	Player p = Player.getDummyInstance();

	/**
	 * the max. possible player movements (distance to init. pos.) depending on 
   * the players first actions.<br> 
   * 
   * The Index:
   * 
   * <ol start=0>
   * <li>playerIndex - [0..10] teammates, [11..21] opponents</li>
	 * <li>cylce</li>
	 * <li>way of interception
	 *   <ol start=0>
   *   <li>forward dashing</li>
   *   <li>turning 1x</li>
   *   <li>turning 2x</li>
   *   <li>turning 3x</li>
   *   <li>backward dashing (direct)</li>
   *   <li>backward dashing after 1x turning</li>
   *   </ol>
   * </li>
   * <ol>
	 */
	public double[][][] playerMovements = new double[22][this.maxCycles][6];

	/**
	 * A class that stores the movements possibilities of all players depending
	 * on their type (heterogeneous players) and stamina
	 * 
	 * @param world -
	 *            the current world-model
	 */
	public Movements(WorldModel world) {

		this.world = world;
		this.teammates = world.getTeammates();
		this.opponents = world.getOpponents();
		this.initPlayerMovements();
	}

	/**
	 * init the player movements.
	 * 
	 */
	private void initPlayerMovements() {

		Arrays.fill(this.lastStamina, 4000.0);
		for (Player p : this.teammates) {
			this.setPlayerMovements(p, true);
		}
		for (Player p : this.opponents) {
			this.setPlayerMovements(p, true);
		}
	}

	/**
	 * sets the players movement (distance from initial position) for the cases:
	 * 
   * <ol start=0>
   *   <li>forward dashing</li>
   *   <li>turning 1x</li>
   *   <li>turning 2x</li>
   *   <li>turning 3x</li>
   *   <li>backward dashing (direct)</li>
   *   <li>backward dashing after 1x turning</li>
   * </ol>
   * It is assumed that the initial player's speed is (0,0). The player's drift
   * has to be calculated seperatly!
   * <br>
   * <br>
	 * <b>Note:</b> a player does not always cover the same distance when dashing
   * or turning first and dashing than as the turning might have a positive 
   * effect on stamina!! As backward dashing consumes twice as much stamina it 
   * is considered seperatly as well.
	 * 
   * @param player - the player
   * @param forceUpdate - if true the matrix is recalculated. This is only 
   * needed when when the player's stamina/effort changes much.
	 */
	public void setPlayerMovements(Player player, boolean forceUpdate) {

		int playerIndex = player.isFriend() ? player.getNumber() - 1
				: 11 + player.getNumber() - 1;

    //no update wanted or needed
		if (!forceUpdate
				&& Math
						.abs(player.getStamina()
								- this.lastStamina[playerIndex]) < this.lastStamina[playerIndex] / 10.0)
			return;

		this.lastStamina[playerIndex] = player.getStamina();

		// only dashing
		this.p.copy(player);
		this.p.getSpeed().reset();

		for (int i = 0; i < this.maxCycles; i++) {
			this.playerMovements[playerIndex][i][0] = this.p
					.getDistance(player);
			this.p.predictThisAfterAction(this.dash100);
		}

		// turning 1x
		this.p.copy(player);
		this.p.getSpeed().reset();

		for (int i = 0; i < this.maxCycles; i++) {
			this.playerMovements[playerIndex][i][1] = this.p
					.getDistance(player);
			if (i == 0)
				this.p.predictThisAfterAction(this.turn180);
			else
				this.p.predictThisAfterAction(this.dash100);
		}

		// turning 2x
		this.p.copy(player);
		this.p.getSpeed().reset();

		for (int i = 0; i < this.maxCycles; i++) {
			this.playerMovements[playerIndex][i][2] = this.p
					.getDistance(player);
			if (i < 2)
				this.p.predictThisAfterAction(this.turn180);
			else
				this.p.predictThisAfterAction(this.dash100);
		}

		// turning 3x
		this.p.copy(player);
		this.p.getSpeed().reset();

		for (int i = 0; i < this.maxCycles; i++) {
			this.playerMovements[playerIndex][i][3] = this.p
					.getDistance(player);
			if (i < 3)
				this.p.predictThisAfterAction(this.turn180);
			else
				this.p.predictThisAfterAction(this.dash100);
		}

		// BackWard
		// only dashing
		this.p.copy(player);
		this.p.getSpeed().reset();

		for (int i = 0; i <= DAInamiteConstants.PROPHET_MAX_BACKWARD
				&& i < this.maxCycles; i++) {
			this.playerMovements[playerIndex][i][4] = this.p
					.getDistance(player);
			this.p.predictThisAfterAction(this.dash100BW);
		}

		// turning 1x
		this.p.copy(player);
		this.p.getSpeed().reset();

		for (int i = 0; i <= DAInamiteConstants.PROPHET_MAX_BACKWARD
				&& i < this.maxCycles; i++) {
			this.playerMovements[playerIndex][i][5] = this.p
					.getDistance(player);
			if (i == 0)
				this.p.predictThisAfterAction(this.turn180);
			else
				this.p.predictThisAfterAction(this.dash100BW);
		}
	}

	/**
	 * this method tells if a player can reach a given position in a given time.
	 * this is an optimized Version for passing (@see PassSituation).
	 * 
	 * @param playerIndex -
	 *            0..10 for team, 11..21 for others
	 * @param cycles -
	 *            max_cylces to reach position
	 * @param target -
	 *            target (ball pos etc.) after max_cycles
	 * @param playerPosition -
	 *            player pos (+ drift after 'cycles' (instead this drift might
	 *            be sub. from the target!))
	 * @param bodyDir -
	 *            players body dir
	 * @param maxTurnAngle1 -
	 *            the maximum turn angle turning once
	 * @param maxTurnAngle2 -
	 *            the maximum turn angle turning twice
	 * @param secureDistance -
	 *            distance ball-player take is taken as secure
	 * @return the {@link INTERCEPTMODE}
	 */
	public ControllerNumbers.INTERCEPTMODE reaches(int playerIndex, int cycles,
			Vektor target, Vektor playerPosition, double bodyDir,
			double maxTurnAngle1, double maxTurnAngle2, double secureDistance) {

		double distance = target.getDistance(playerPosition);

		// point is much too far away, such that max-speed wouldn't
		// reach it!
		double spdMax = world.getSelfRef().getPConf().PLAYER_SPEED_MAX;
		if ((distance - secureDistance) / spdMax > cycles) {
			return INTERCEPTMODE.NO_INTERCEPT;
		} 
    else if (distance < this.playerMovements[playerIndex][cycles][3]
				+ secureDistance) {
			// player gets ball even when turning 3x (slowest poss.)
			return ControllerNumbers.INTERCEPTMODE.TURNING3;
		} 
    else {
			double angle;
			// dashing
			if (distance < this.playerMovements[playerIndex][cycles][0]
					+ secureDistance) {
				angle = Math.atan2(target.x - playerPosition.x, target.y
						- playerPosition.y)
						* (180.0 / Math.PI);
				angle = Math.abs(Vektor.normalize(bodyDir - angle));
				if (Math.abs(Math.sin(Math.toRadians(angle))) * distance < secureDistance) {
					if (angle > 90.0
							&& cycles <= DAInamiteConstants.PROPHET_MAX_BACKWARD) {
						return ControllerNumbers.INTERCEPTMODE.DASH_BW;
					}
					if (angle <= 90.0) {
						return ControllerNumbers.INTERCEPTMODE.DASH_FW;
					}
				}
			} 
      else {
				return ControllerNumbers.INTERCEPTMODE.NO_INTERCEPT;
			}

			// turning 1x
			if (distance < this.playerMovements[playerIndex][cycles][1]
					+ secureDistance) {
				if (angle < maxTurnAngle1) {
					return ControllerNumbers.INTERCEPTMODE.TURNING1_FW;
				}
				// backward
				if (180.0 - angle < maxTurnAngle1
						&& cycles <= DAInamiteConstants.PROPHET_MAX_BACKWARD) {
					return ControllerNumbers.INTERCEPTMODE.TURNING1_BW;
				}
			} 
      else {
				return ControllerNumbers.INTERCEPTMODE.NO_INTERCEPT;
			}

			// turning 2x
			if (distance < this.playerMovements[playerIndex][cycles][2]
					+ secureDistance) {
				if (angle < maxTurnAngle2) {
					return ControllerNumbers.INTERCEPTMODE.TURNING2;
				}
			}
		}
		return ControllerNumbers.INTERCEPTMODE.NO_INTERCEPT;
	}

	/**
	 * this method tells if a player can reach a given position in a given time.
	 * this is an optimized Version for passing (@see PassSituation).
	 * 
	 * @param playerIndex -
	 *            0..10 for team, 11..21 for others
	 * @param cycles -
	 *            max_cylces to reach position
	 * @param target -
	 *            target (ball pos etc.) to reach after <code>cycles</code>
	 * @param playerPosition -
	 *            player pos (+ drift after <code>cycles</code> (instead this 
   *            drift might be sub. from the target!))
	 * @param bodyDir -
	 *            players body dir
	 * @param maxTurnAngle1 -
	 *            the maximum turn angle turning once
	 * @param maxTurnAngle2 -
	 *            the maximum turn angle turning twice
	 * @param secureDistance -
	 *            distance ball-player that is taken as secure (i.e. 
   *            kick-distance)
	 * @param tolerance - tolerance in % (i.e. 5 meaning that the player reaches
   * an average of 95% of the max. possible velocity when going to the point,
   * loosing 5% due to turning) 
	 * @return the distance left when the player tries to reach the given
	 *         position. distance > 0 means the point will not be reached
	 */
	public double getLeftDistance(int playerIndex, int cycles, Vektor target,
			Vektor playerPosition, double bodyDir, double maxTurnAngle1,
			double maxTurnAngle2, double secureDistance, double tolerance) {

		double distance = target.getDistance(playerPosition);
		double angle = Math.atan2(target.x - playerPosition.x, target.y
				- playerPosition.y)
				* (180.0 / Math.PI);
		angle = Math.abs(Vektor.normalize(angle - bodyDir));

		// dash
		if (Math.abs(Math.sin(Math.toRadians(angle))) * distance < secureDistance) {
			return distance - this.playerMovements[playerIndex][cycles][0]
					* tolerance - secureDistance;
		}
		// 1x turning
		else if (angle < maxTurnAngle1) {
			return distance - this.playerMovements[playerIndex][cycles][1]
					* tolerance - secureDistance;
		}
		// 2x turning
		else if (angle < maxTurnAngle2) {
			return distance - this.playerMovements[playerIndex][cycles][2]
					* tolerance - secureDistance;
		}
		// 3x turning
		else {
			return distance - this.playerMovements[playerIndex][cycles][3]
					* tolerance - secureDistance;
		}
	}
}
